/**********************************************************************//**
 @file ptcloud.cpp 
 @brief Contains methods for modifying and inquiring Point Cloud information.
 @author Ricardo C. Marques <rmarques@inf.puc-rio.br>
 @version 0.0.1
 @date 05/11/2012
**************************************************************************/
// Standard Libraries
#define  _CRT_SECURE_NO_WARNINGS
#include <stdlib.h>
#include <stdio.h>
#undef   _CRT_SECURE_NO_WARNINGS
#define  _USE_MATH_DEFINES
#include <math.h>
#undef   _USE_MATH_DEFINES


// Module Own Header File
#include "ptcloud.hpp"

// Auxiliary Header Files
#include "draw.hpp"

PointCloud::PointCloud(Point3D* points, size_t num_points)
{
    // Copy points coordinates
    m_NumPoints = num_points;
    m_Points = (Point3D*) malloc(m_NumPoints*sizeof(Point3D));
    for(size_t i=0; i<m_NumPoints; i++)
    {
        m_Points[i].x = points[i].x;
        m_Points[i].y = points[i].y;
        m_Points[i].z = points[i].z;
    }

    // Compute Centroid
    ComputeCentroid();
    
}
PointCloud::PointCloud(const char* filename)
{
    m_NumPoints = 0;
    m_Points = NULL;
    m_Centroid.x = 0.;
    m_Centroid.y = 0.;

    FILE* fp = fopen(filename, "rt");
    if( fp==NULL )
    {
        printf("Error: PointCloud could not open file: %s!!!\n", filename);
        exit(1);
    }

    if( fscanf(fp, "%d", &m_NumPoints)!=1 )
    {
        printf("Error: PointCloud could not read number of points in file: %s!!!\n", filename);
        exit(1);
    }

    m_Points = (Point3D*) malloc(m_NumPoints*sizeof(Point3D));
    for(size_t i=0; i<m_NumPoints; i++)
    {
        if( fscanf(fp, "%lf %lf %lf", &m_Points[i].x, &m_Points[i].y, &m_Points[i].z)!=3)
        {
            printf("Warning: PointCloud could not read point in line %d", i+2);
        }
    }

    // Compute Centroid
    ComputeCentroid();

}
PointCloud::~PointCloud()
{
    free(m_Points);
}
void PointCloud::ComputeCentroid(void)
{
    m_Centroid.x = 0.;
    m_Centroid.y = 0.;
    for(size_t i=0; i<m_NumPoints; i++)
    {
        m_Centroid.x += m_Points[i].x;
        m_Centroid.y += m_Points[i].y;
    }
    m_Centroid.x /= m_NumPoints;
    m_Centroid.y /= m_NumPoints;
}
void PointCloud::GetCentroid(Point2D* centroid)
{
    centroid->x = m_Centroid.x;
    centroid->y = m_Centroid.y;
}
void PointCloud::DrawPointCloud(void)
{
    DrawPoints( (float*)m_Points, m_NumPoints, 4.f, 0.f,1.f,0.f);
}
void PointCloud::GetBoundingBox(Point2D* min, Point2D* max)
{
    min->x = m_Points[0].x;
    min->y = m_Points[0].y;
    max->x = m_Points[0].x;
    max->y = m_Points[0].y;

    for(size_t i=1; i<m_NumPoints; i++)
    {
        if( m_Points[i].x < min->x )
            min->x = m_Points[i].x;
        if( m_Points[i].y < min->y )
            min->y = m_Points[i].y;
        if( m_Points[i].x > max->x )
            max->x = m_Points[i].x;
        if( m_Points[i].y > max->y )
            max->y = m_Points[i].y;
    }
}
void PointCloud::GetBoundingTri( Point2D* T1, Point2D* T2, Point2D* T3 )
{
    Point2D min={0.,0.};
    Point2D max={0.,0.};
    GetBoundingBox( &min, &max );

    double deltaX = max.x - min.x;
    double deltaY = max.y - min.y;
    double M = 0;
    if( deltaX < deltaY )
        M = deltaY;
    else
        M = deltaX;

#if _DEBUG
    Point2D centroid = { (min.x+max.x)/2, (min.y+max.y)/2 };
    Point2D diff={ m_Centroid.x-centroid.x, m_Centroid.y-centroid.y };
    printf("diff_centroids={%lf,%lf}\n", diff.x, diff.y );
#endif

    T1->x = m_Centroid.x + 3*M;
    T1->y = m_Centroid.y;

    T2->x = m_Centroid.x;
    T2->y = m_Centroid.y + 3*M;

    T3->x = m_Centroid.x - 3*M;
    T3->y = m_Centroid.y - 3*M;
}
